#include "../../include/hardware/motor.h"
template <typename T>
inline T motor::float2int(float in_data, uint8_t type)
{
    switch (type)
    {
    case 0: // radian float pos to uint16_t   RS01/02（-12.57f-12.57f）
        return (uint16_t)((in_data +12.57 ) * 2606.76);
        
    case 1: // radian float vel to uint16_t   RS01/02（（-44rad/s-44rad/s）
        return (uint16_t)((in_data+44)*744.704 );

    //TODO    
    case 2: // angle float pos to uint16_t
        return (uint16_t)(in_data * 360.0 / 100000.0);
    case 3: // angle float vel to uint16_t
        return (uint16_t)(in_data * 360.0 / 100000.0);


    case 4: // float torque to uint16_t RS01 RS02******************************
        return (uint16_t)((in_data +17)*1927.47);

    

    case 6: // float Kp to uint16 RS01/RS02/RS00/DM4340     0~500
        return (uint16_t)(in_data*131.07);
    case 7: // float Kd to uint16 RS01/RS02/RS00/DM4340     0~5
        return (uint16_t)(in_data*13107);

    //---------------------------------RS00---------------------------------------
    case 8: // radian float vel to uint16_t     （-33rad/s~33rad/s）
        return (uint16_t)((in_data+33)*992.94);
    case 9: // float torque to uint16_t         （-14nm~14nm）
        return (uint16_t)((in_data +14)*2340.5);
    //---------------------------------RS00---------------------------------------

    //---------------------------------RS03---------------------------------------
    case 10: // radian float vel to uint16_t     （-20rad/s-20rad/s）
        return (uint16_t)((in_data+20)*1638.35);
    case 11: // float torque to uint16_t         （-60nm~60nm）
        return (uint16_t)((in_data +60)*546.116);
    case 12: // float Kp to uint16 RS03/RS04        0~5000
        return (uint16_t)(in_data*13.107);
    case 13: // float Kd to uint16 RS03/RS04        0~100
        return (uint16_t)(in_data*655.35);
    //---------------------------------RS03---------------------------------------

    //---------------------------------RS04---------------------------------------
    case 14: // radian float vel to uint16_t     （-15rad/s~15rad/s）
        return (uint16_t)((in_data+15)*2184.5);
    case 15: // float torque to uint16_t         （-120nm~120nm）
        return (uint16_t)((in_data +120)*273.058);
    //---------------------------------RS04---------------------------------------

    //---------------------------------DM4340---------------------------------------
    case 16: // radian float vel to uint16_t     （-30rad/s~30rad/s）
        return (uint16_t)((in_data+30)*1092.23);
    case 17: // float torque to uint16_t         （-27nm~27nm）
        return (uint16_t)((in_data +27)*1213.6);
    //---------------------------------DM4340---------------------------------------

    default:
        return T();
    }
}

inline float motor::int2float(uint16_t in_data, uint8_t type)
{
    switch (type)
    {
    case 0: // radian float pos to uint16_t   RS01/02（-12.57f-12.57f）
        return (float)in_data * 0.000383-12.57;
        //return (float)(in_data * my_2pi / 100000.0);
    case 1: // radian float vel to uint16_t   RS01/02（（-44rad/s-44rad/s）
        return (float)(in_data *0.001342-44);
        //return (float)(in_data * my_2pi / 100000.0);
        
    case 2: // angle float pos to uint16_t
        return (float)(in_data * 360.0 / 100000.0);
    case 3: // angle float vel to uint16_t
        return (float)(in_data * 360.0 / 100000.0);


    case 4: // float torque to uint16_t RS01 RS02******************************
        return (float)(in_data * 0.000518-17);
    case 5: // float torque to uint16_t 4538
        return (float)(in_data *0.000445);

    
    //---------------------------------RS00---------------------------------------
    case 6: // radian float vel to uint16_t   （-33rad/s~33rad/s）
        return (float)(in_data *0.001007-33);
    case 7: // float torque to uint16_t        (-14nm~14nm）
        return (float)(in_data * 0.0004273-14);
    //---------------------------------RS00---------------------------------------

    //---------------------------------RS03---------------------------------------
    case 8: // radian float vel to uint16_t   （-20rad/s~20rad/s）
        return (float)(in_data *0.00061037-20);
    case 9: // float torque to uint16_t        (-60nm~60nm）
        return (float)(in_data * 0.001831-60);
    //---------------------------------RS03---------------------------------------

    //---------------------------------RS04---------------------------------------
    case 10: // radian float vel to uint16_t   （-15rad/s~15rad/s）
        return (float)(in_data *0.0004578-15);
    case 11: // float torque to uint16_t        (-120nm~120nm）
        return (float)(in_data * 0.003662-120);
    //---------------------------------RS04---------------------------------------

    //---------------------------------DM4340---------------------------------------
    case 12: // radian float vel to uint16_t   （-30rad/s~30rad/s）
        return (float)(in_data *0.0009155-30);
    case 13: // float torque to uint16_t        (-27nm~27nm）
        return (float)(in_data * 0.000824-27);
    //---------------------------------DM4340---------------------------------------

    default:
        return float();

    }
}

void motor::fresh_cmd(float position, float velocity, float torque, float Kp, float Kd)
{
    switch(type_)
    {
        case motor_type::null:
            ROS_INFO("error,motor type not set,fresh command fault");
            break;
        case motor_type::RS00:
            cmd.motor_cmd.position = float2int<uint16_t>(position, 0);
            cmd.motor_cmd.velocity = float2int<uint16_t>(velocity, 8);
            cmd.motor_cmd.torque = float2int<uint16_t>(torque, 9);
            cmd.motor_cmd.Kp = float2int<uint16_t>(Kp, 6);
            cmd.motor_cmd.Kd = float2int<uint16_t>(Kd, 7);
        case motor_type::RS01:
            cmd.motor_cmd.position = float2int<uint16_t>(position, 0);
            cmd.motor_cmd.velocity = float2int<uint16_t>(velocity, 1);
            cmd.motor_cmd.torque = float2int<uint16_t>(torque, 4);
            cmd.motor_cmd.Kp = float2int<uint16_t>(Kp, 6);
            cmd.motor_cmd.Kd = float2int<uint16_t>(Kd, 7);
            break;
        case motor_type::RS02:
            cmd.motor_cmd.position = float2int<uint16_t>(position, 0);
            cmd.motor_cmd.velocity = float2int<uint16_t>(velocity, 1);
            cmd.motor_cmd.torque = float2int<uint16_t>(torque, 4);
            cmd.motor_cmd.Kp = float2int<uint16_t>(Kp, 6);
            cmd.motor_cmd.Kd = float2int<uint16_t>(Kd, 7);
            break;
        case motor_type::RS03:
            cmd.motor_cmd.position = float2int<uint16_t>(position, 0);
            cmd.motor_cmd.velocity = float2int<uint16_t>(velocity, 10);
            cmd.motor_cmd.torque = float2int<uint16_t>(torque, 11);
            cmd.motor_cmd.Kp = float2int<uint16_t>(Kp, 12);
            cmd.motor_cmd.Kd = float2int<uint16_t>(Kd, 13);
            break;    
        case motor_type::RS04:
            cmd.motor_cmd.position = float2int<uint16_t>(position, 0);
            cmd.motor_cmd.velocity = float2int<uint16_t>(velocity, 14);
            cmd.motor_cmd.torque = float2int<uint16_t>(torque, 15);
            cmd.motor_cmd.Kp = float2int<uint16_t>(Kp, 12);
            cmd.motor_cmd.Kd = float2int<uint16_t>(Kd, 13);
            break;
        case motor_type::DM4340:
            cmd.motor_cmd.position = float2int<uint16_t>(position, 0);
            cmd.motor_cmd.velocity = float2int<uint16_t>(velocity, 16);
            cmd.motor_cmd.torque = float2int<uint16_t>(torque, 17);
            cmd.motor_cmd.Kp = float2int<uint16_t>(Kp, 6);
            cmd.motor_cmd.Kd = float2int<uint16_t>(Kd, 7);
            break;

        default:
            ROS_INFO("motor type setting error");
            return;
    }
    if(cmd.motor_cmd.Kp<0||cmd.motor_cmd.Kd<0)return;
    
    cmd.crc16 = crc_ccitt(0x0000, (const uint8_t *)&cmd, sizeof(cdc_acm_rx_message_t) - 2);
    // ROS_INFO("CRC: 0x%x",cmd.crc16);
}

//解包
void motor::fresh_data(uint16_t position, uint16_t velocity, uint16_t torque)
{
    switch(type_)
    {
        case motor_type::null:
            ROS_INFO("error,motor type not set,fresh data fault");
            break;
        case motor_type::RS00:
            p_msg.pos = data.position = int2float(position, 0);
            p_msg.vel = data.velocity = int2float(velocity, 6);
            p_msg.tau = data.torque = int2float(torque, 7);
            break;
        case motor_type::RS01:
            p_msg.pos = data.position = int2float(position, 0);
            p_msg.vel = data.velocity = int2float(velocity, 1);
            p_msg.tau = data.torque = int2float(torque, 4);
            break;
        case motor_type::RS02:
            p_msg.pos = data.position = int2float(position, 0);
            p_msg.vel = data.velocity = int2float(velocity, 1);
            p_msg.tau = data.torque = int2float(torque, 4);
            break;
        case motor_type::RS03:
            p_msg.pos = data.position = int2float(position, 0);
            p_msg.vel = data.velocity = int2float(velocity, 8);
            p_msg.tau = data.torque = int2float(torque, 9);
            break;
        case motor_type::RS04:
            p_msg.pos = data.position = int2float(position, 0);
            p_msg.vel = data.velocity = int2float(velocity, 10);
            p_msg.tau = data.torque = int2float(torque, 11);
            break;
        case motor_type::DM4340:
            p_msg.pos = data.position = int2float(position, 0);
            p_msg.vel = data.velocity = int2float(velocity, 12);
            p_msg.tau = data.torque = int2float(torque, 13);
            break;
        
        default:
            ROS_INFO("motor type setting error");
            return;
    }
    //ROS_INFO("_motor_pub");
    _motor_pub.publish(p_msg);
}